#include "Camera.hpp"

namespace zzz{

void Camera::SetPerspective(const int cw, const int ch, const double near, const double far, const double angle)
{
  if (near != -1) zNear_=near;
  if (far != -1) zFar_=far;
  if (angle != -1) fovy_=angle;
  if (ch==0) return;
  width_=cw;
  height_=ch;
  aspect_=(double)cw/(double)ch;
  glMatrixMode(GL_PROJECTION); 
  glLoadIdentity();
  if (!orthoMode_)
    gluPerspective(fovy_,aspect_,zNear_,zFar_);
  else {
    double r=orthoScale_;
    double t=r/aspect_;
    GLTransformationd trans;
    trans.Identical();
    trans(0,0)=1.0/r;
    trans(1,1)=1.0/t;
    trans(2,2)=-2.0/100000; //(zFar_-zNear_);
    trans(2,3)=0; //-(zFar_+zNear_)/(zFar_-zNear_);
    trans.ApplyGL();
  }
  glMatrixMode(GL_MODELVIEW); 

}


void Camera::OffsetPosition(const double &x, const double &y, const double &z)
{
  Position_[0]+=x;
  Position_[1]+=y;
  Position_[2]+=z;
}


void Camera::SetPosition(const double &x, const double &y, const double &z)
{
  Position_[0]=x;
  Position_[1]=y;
  Position_[2]=z;
}


void Camera::OffsetPosition(const Vector<3,double> &offset)
{
  Position_+=offset;
}

void Camera::Update()
{
  Quaterniond q;

  // Make the Quaternions that will represent our rotations
  qPitch_.SetAxisAngle(Vector<3,double>(1,0,0),PitchDegrees_);
  qHeading_.SetAxisAngle(Vector<3,double>(0,1,0),YawDegrees_);

  // Combine the pitch and heading rotations and store the results in q
  q = qPitch_ * qHeading_;
  Transform_=GLTransformationd(Rotationd(q));

  //original forward is 0,0,-1
  DirectionVector_=q.RotateBackVector(Vector3d(0,0,-1));
}


void Camera::SetPosition(const Vector<3,double> &pos)
{
  Position_=pos;
}

// positive: forwards, negative: backwards

void Camera::MoveForwards(double dist)
{
  // Increment our position by the vector
  Position_ += DirectionVector_*dist;
}

// positive: leftwards, negative: rightwards

void Camera::MoveLeftwards(double dist)
{
  Quaternion<double> toHead(DirectionVector_.Cross(Vector3d(0,1,0)),PI/2);
  Vector3d newhead=toHead.RotateVector(DirectionVector_);
  Quaternion<double> toSide(newhead,PI/2);
  Vector3d LeftDir=toSide.RotateVector(DirectionVector_);
  // Increment our position by the vector
  Position_ += LeftDir*dist;
}

void Camera::MoveUpwards(double dist)
{
  Quaternion<double> toHead(DirectionVector_.Cross(Vector3d(0,1,0)),PI/2);
  Vector3d newhead=toHead.RotateVector(DirectionVector_);
  // Increment our position by the vector
  Position_ += newhead*dist;
}

void Camera::ChangeYaw(double degrees)
{
  if(Abs(degrees) < Abs(MaxYawRate_)) {
    // Our Heading is less than the max heading rate that we 
    // defined so lets increment it but first we must check
    // to see if we are inverted so that our heading will not
    // become inverted.
    if(PitchDegrees_ > 90*C_D2R && PitchDegrees_ < 270*C_D2R || (PitchDegrees_ < -90*C_D2R && PitchDegrees_ > -270*C_D2R))
      YawDegrees_ -= degrees;
    else
      YawDegrees_ += degrees;
  } else {
    // Our heading is greater than the max heading rate that
    // we defined so we can only increment our heading by the 
    // maximum allowed value.
    if(degrees < 0) {
      // Check to see if we are upside down.
      if((PitchDegrees_ > 90*C_D2R && PitchDegrees_ < 270*C_D2R) || (PitchDegrees_ < -90*C_D2R && PitchDegrees_ > -270*C_D2R)) {
        // Ok we would normally decrement here but since we are upside
        // down then we need to increment our heading
        YawDegrees_ += MaxYawRate_;
      } else {
        // We are not upside down so decrement as usual
        YawDegrees_ -= MaxYawRate_;
      }
    } else {
      // Check to see if we are upside down.
      if(PitchDegrees_ > 90*C_D2R && PitchDegrees_ < 270*C_D2R || (PitchDegrees_ < -90*C_D2R && PitchDegrees_ > -270*C_D2R)) {
        // Ok we would normally increment here but since we are upside
        // down then we need to decrement our heading.
        YawDegrees_ -= MaxYawRate_;
      } else {
        // We are not upside down so increment as usual.
        YawDegrees_ += MaxYawRate_;
      }
    }
  }

  // We don't want our heading to run away from us either. Although it
  // really doesn't matter I prefer to have my heading degrees
  // within the range of -360.0f to 360.0f
  if(YawDegrees_ > 360*C_D2R) {
    YawDegrees_ -= 360*C_D2R;
  } else if(YawDegrees_ < -360*C_D2R) {
    YawDegrees_ += 360*C_D2R;
  }

  Update();
}


void Camera::ChangePitch(double degrees)
{
  if(Abs(degrees) < Abs(MaxPitchRate_)) {
    // Our pitch is less than the max pitch rate that we 
    // defined so lets increment it.
    PitchDegrees_ += degrees;
  } else {
    // Our pitch is greater than the max pitch rate that
    // we defined so we can only increment our pitch by the 
    // maximum allowed value.
    if(degrees < 0) {
      // We are pitching down so decrement
      PitchDegrees_ -= MaxPitchRate_;
    } else {
      // We are pitching up so increment
      PitchDegrees_ += MaxPitchRate_;
    }
  }

  // We don't want our pitch to run away from us. Although it
  // really doesn't matter I prefer to have my pitch degrees
  // within the range of -360.0f to 360.0f
  if(PitchDegrees_ > 360*C_D2R) {
    PitchDegrees_ -= 360*C_D2R;
  } else if(PitchDegrees_ < -360*C_D2R) {
    PitchDegrees_ += 360*C_D2R;
  }
  Update();
}


void Camera::ApplyGL(void) const
{
  // Let OpenGL set our new prespective on the world!
  Transform_.ApplyGL();

  // Translate to our new position.
  glTranslated(-Position_[0],-Position_[1],-Position_[2]);
}

void Camera::Reset()
{
  // Initalize all our member varibles.
  MaxPitchRate_      = 5;
  MaxYawRate_    = 5;
  YawDegrees_    = 0;
  PitchDegrees_      = 0;
  Position_.Set(0,0,0);
  Update();
}

Camera::Camera()
:zNear_(0.01), zFar_(1000), fovy_(60), orthoScale_(1.0), orthoMode_(false)
{
  Reset();
}

zzz::GLTransformationd Camera::GetGLTransformation()
{
  GLTransformationd trans(Translationd(-Position_[0], -Position_[1], -Position_[2]));
  trans = trans * Transform_;
  return trans;
}

void Camera::LookAt(const Vector3d &from, const Vector3d &to, const Vector3d &up)
{
  Vector3d f = to - from;
  f.Normalize();
  Vector3d s = Cross(f, up.Normalized());
  Vector3d u = Cross(s, f);
  Rotationd rot(Matrix3x3d(s,u,-f));
  Transform_.Set(rot, Translationd(0,0,0));

  // hopefully correct, roll must be zero
  double roll;
  rot.ToEulerAngles(PitchDegrees_, YawDegrees_, roll);
  PitchDegrees_*=C_R2D;
  YawDegrees_*=C_R2D;
}

void Camera::SetOrthoMode(bool mode)
{
  orthoMode_=mode;
  SetPerspective(width_, height_);
}

}